#include <iostream>
#include <string>
#include <vector>

#include "ros/ros.h"
#include "sensor_msgs/JointState.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
#include "kdl_parser/kdl_parser.hpp"
#include "kdl/chainiksolverpos_nr_jl.hpp"
#include "trac_ik/trac_ik.hpp"
#include "urdf/model.h"
#include <math.h>
#include <serial/serial.h>
#include "geometry_msgs/PointStamped.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "std_msgs/Float64.h" 

#define pi 3.141592653

//前后左右停:a,b,c,d,e

char cmd='a';

int main(int argc,char* argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"gou2_test");
    ros::NodeHandle nh;
    ros::Publisher joint_pub=nh.advertise<sensor_msgs::JointState>("joint_states",1000);
    
    ros::Publisher lf_jian_pu=nh.advertise<std_msgs::Float64>("/gou2/lf_jian_con/command",1000);
    ros::Publisher lf_zhou_pu=nh.advertise<std_msgs::Float64>("/gou2/lf_zhou_con/command",1000);
    ros::Publisher lf_xi_pu=nh.advertise<std_msgs::Float64>("/gou2/lf_xi_con/command",1000);
    ros::Publisher lb_jian_pu=nh.advertise<std_msgs::Float64>("/gou2/lb_jian_con/command",1000);
    ros::Publisher lb_zhou_pu=nh.advertise<std_msgs::Float64>("/gou2/lb_zhou_con/command",1000);
    ros::Publisher lb_xi_pu=nh.advertise<std_msgs::Float64>("/gou2/lb_xi_con/command",1000);
    ros::Publisher rf_jian_pu=nh.advertise<std_msgs::Float64>("/gou2/rf_jian_con/command",1000);
    ros::Publisher rf_zhou_pu=nh.advertise<std_msgs::Float64>("/gou2/rf_zhou_con/command",1000);
    ros::Publisher rf_xi_pu=nh.advertise<std_msgs::Float64>("/gou2/rf_xi_con/command",1000);
    ros::Publisher rb_jian_pu=nh.advertise<std_msgs::Float64>("/gou2/rb_jian_con/command",1000);
    ros::Publisher rb_zhou_pu=nh.advertise<std_msgs::Float64>("/gou2/rb_zhou_con/command",1000);
    ros::Publisher rb_xi_pu=nh.advertise<std_msgs::Float64>("/gou2/rb_xi_con/command",1000);

    //定义tf坐标广播器
    tf2_ros::StaticTransformBroadcaster broadcaster;
    geometry_msgs::TransformStamped ts;

    ts.header.seq=100;
    ts.header.stamp=ros::Time::now();
    ts.header.frame_id="world";
    ts.child_frame_id="base_link";
    ts.transform.translation.x=0.0;
    ts.transform.translation.y=0.0;
    ts.transform.translation.z=0.0805;

    tf2::Quaternion qtn;
    qtn.setRPY(0,0,0);
    ts.transform.rotation.x=qtn.getX();
    ts.transform.rotation.y=qtn.getY();
    ts.transform.rotation.z=qtn.getZ();
    ts.transform.rotation.w=qtn.getW();
    broadcaster.sendTransform(ts);
/*
    //创建一个serial对象
    serial::Serial sp;
    serial::Timeout to = serial::Timeout::simpleTimeout(10);
    //设置要打开的串口名称
    sp.setPort("/dev/ttyUSB0");
    //设置串口通信的波特率
    sp.setBaudrate(9600);
    //串口设置timeout
    sp.setTimeout(to);

    try
    {
        //打开串口
        sp.open();
    }
    catch(serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port.");
        return -1;
    }
    
    //判断串口是否打开成功
    if(sp.isOpen())
    {
        ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
    }
    else
    {
        return -1;
    }
*/
    sensor_msgs::JointState joint_state;
    
    std::string robot_desc_string;
    nh.param("robot1_description", robot_desc_string, std::string());
    KDL::Tree my_tree;
    if(!kdl_parser::treeFromString(robot_desc_string, my_tree))
    //if(!kdl_parser::treeFromFile("/home/zhitong/catkin_ws_serial/src/Gou2/urdf/Gou2.urdf", my_tree))
    {
        ROS_ERROR("Failed to construct kdl tree");
    }
    else
    {
        ROS_INFO("成功生成kdl树!");
    }
    std::vector<std::string> joint_name = {
"LF_JIAN_JOINT", "LF_ZHOU_JOINT", "LF_XI_JOINT", "LF_R_JOINT","LF_P_JOINT", "LF_Y_JOINT",
"LB_JIAN_JOINT", "LB_ZHOU_JOINT", "LB_XI_JOINT", "LB_R_JOINT","LB_P_JOINT", "LB_Y_JOINT",
"RF_JIAN_JOINT", "RF_ZHOU_JOINT", "RF_XI_JOINT", "RF_R_JOINT","RF_P_JOINT", "RF_Y_JOINT",
"RB_JIAN_JOINT", "RB_ZHOU_JOINT", "RB_XI_JOINT", "RB_R_JOINT","RB_P_JOINT", "RB_Y_JOINT"
};
    std::vector<double> joint_pos = {
0,0,0,0,0,0,0,
0,0,0,0,0,0,0,
0,0,0,0,0,0,0,
0,0,0,0,0,0,0
};
    
    std::string urdf_param = "/robot1_description";
    double timeout = 0.005;
    double eps = 1e-5;
    std::string chain_start = "base_link"; 
    std::string chain_end = "LF_Y_LINK"; 
    TRAC_IK::TRAC_IK tracik_solver(chain_start, chain_end, urdf_param, timeout, eps);
    KDL::Chain chain;
    KDL::JntArray ll, ul; //关节下限, 关节上限
    bool valid = tracik_solver.getKDLChain(chain);
    if(!valid)
    {
        ROS_ERROR("There was no valid KDL chain found");
    }
    valid = tracik_solver.getKDLLimits(ll, ul);
    if(!valid)
    {
        ROS_ERROR("There was no valid KDL joint limits found");
    }
    KDL::ChainFkSolverPos_recursive fk_solver(chain);
    ROS_INFO("关节数量: %d", chain.getNrOfJoints());

    KDL::JntArray q(6); // 初始关节位置
    q(0)=0;
    q(1)=-pi/6;
    q(2)=pi/6;
    q(3)=0;
    q(4)=0;
    q(5)=0;
    //定义末端4x4齐次变换矩阵(各个关节位于初始位置时，末端齐次矩阵)
    KDL::Frame end_effector_pose1_l;
    KDL::Frame end_effector_pose1_r;
    //定义末端4x4齐次变换矩阵(各个关节的实时末端齐次矩阵)
    KDL::Frame end_effector_pose_l;//left now
    KDL::Frame end_effector_pose_r;//left now

    //定义逆运动学解算结果存储数组
    KDL::JntArray result_l;//left
    KDL::JntArray result_r;//right
    //上一帧的关节变量
    KDL::JntArray resu_last_l;//left last
    KDL::JntArray resu_last_r;//right last
    ros::Rate r(40);

    auto print_frame_lambda = [](KDL::Frame f)
    {
        double x, y, z, roll, pitch, yaw;
        x = f.p.x();
        y = f.p.y();
        z = f.p.z();
        f.M.GetRPY(roll, pitch, yaw);
        std::cout << "x:" << x << " y:" << y << " z:" << z << " roll:" << roll << " pitch:" << pitch << " yaw:" << yaw << std::endl;
    };

    //正运动学求初始状态齐次变换矩阵
    fk_solver.JntToCart(q,end_effector_pose1_l);
    fk_solver.JntToCart(q,end_effector_pose1_r);
    print_frame_lambda(end_effector_pose1_l);
    
    ROS_INFO("更新关节状态");
    joint_state.header.stamp = ros::Time::now();
    //ROS_INFO("%d",joint_state.header.stamp);
    joint_state.name.resize(24);
    joint_state.position.resize(24);

    for(size_t i=0;i<=23;i++)
    {
        joint_state.name[i] = joint_name[i];
    }
    
    for(int i=0;i<=23;i++)
    {
        joint_state.position[i]=q(i%6);
    }
    joint_pub.publish(joint_state);

    fk_solver.JntToCart(q, end_effector_pose_l);
    fk_solver.JntToCart(q, end_effector_pose_r);

    resu_last_l=q;
    resu_last_r=q;

    double r_b=0.0,p_b=0.0,y_b=0.0;

    geometry_msgs::PointStamped point_base;
    point_base.header.frame_id="base_link";
    point_base.header.stamp=ros::Time::now();
    point_base.point.x=0;
    point_base.point.y=0;
    point_base.point.z=0;

    tf2_ros::Buffer BUffer;
    tf2_ros::TransformListener listener(BUffer);

    while(ros::ok())
    {
    cmd='a';

/*
        size_t n = sp.available();
        if(n!=0)
        {
            uint8_t buffer[1024];
            n = sp.read(buffer, n);
            cmd=buffer[0];
        }
    
*/

    if(cmd=='a')
    {
    //x=0.02*(t-sint);
    //y=0.02*(1-cost);
    for(int i=1;i<=20;i++)
    { 
        double t=2*pi*i/20;
        double x=0.005*(t-sin(t));
        double z=0.005*(1-cos(t));
        end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+x;
        end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2]+z;
        int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);

        end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+0.005*2*pi-x;
        end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2];
        int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);

	//ROS_INFO("%f,%f,%f,%f,%f,%f",result_l(0),result_l(1),result_l(2),result_l(3),result_l(4),result_l(5));
 
        joint_state.header.stamp = ros::Time::now();

        for(int j=0;j<=23;j++)
        {
            joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
        }
        joint_pub.publish(joint_state);


        std_msgs::Float64 joint_po;
        
        joint_po.data=joint_state.position[0];
        lf_jian_pu.publish(joint_po);
        joint_po.data=joint_state.position[1];
        lf_zhou_pu.publish(joint_po);
        joint_po.data=joint_state.position[2];
        lf_xi_pu.publish(joint_po);

        joint_po.data=joint_state.position[6];
        lb_jian_pu.publish(joint_po);
        joint_po.data=joint_state.position[7];
        lb_zhou_pu.publish(joint_po);
        joint_po.data=joint_state.position[8];
        lb_xi_pu.publish(joint_po);

        joint_po.data=joint_state.position[12];
        rf_jian_pu.publish(joint_po);
        joint_po.data=joint_state.position[13];
        rf_zhou_pu.publish(joint_po);
        joint_po.data=joint_state.position[14];
        rf_xi_pu.publish(joint_po);

        joint_po.data=joint_state.position[18];
        rb_jian_pu.publish(joint_po);
        joint_po.data=joint_state.position[19];
        rb_zhou_pu.publish(joint_po);
        joint_po.data=joint_state.position[20];
        rb_xi_pu.publish(joint_po);




        ts.header.stamp=ros::Time::now();
        point_base.header.stamp=ros::Time::now();
        point_base.point.x=0.00025*2*pi;
        point_base.point.y=0;
        point_base.point.z=0;

        geometry_msgs::PointStamped point_world;
        point_world=BUffer.transform(point_base,"world");

        ts.transform.translation.x=point_world.point.x;
        ts.transform.translation.y=point_world.point.y;
        ts.transform.translation.z=point_world.point.z;

        point_base.point.x=0;



        ts.transform.translation.x+=0.00025*2*pi;
        ts.transform.translation.y=0.0;
        ts.transform.translation.z=0.0805;


        qtn.setRPY(r_b,p_b,y_b);
        ts.transform.rotation.x=qtn.getX();
        ts.transform.rotation.y=qtn.getY();
        ts.transform.rotation.z=qtn.getZ();
        ts.transform.rotation.w=qtn.getW();
        broadcaster.sendTransform(ts);

        resu_last_l=result_l;
        resu_last_r=result_r;
        r.sleep();
    }
    
    for(int i=1;i<=20;i++)
    {
        double t=2*pi*i/20;
        double x=0.005*(t-sin(t));
        double z=0.005*(1-cos(t));
        end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+0.005*2*pi-x;
        end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2];
        int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);

        end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+x;
        end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2]+z;
        int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);
    
        joint_state.header.stamp = ros::Time::now();

        for(int j=0;j<=23;j++)
        {
            joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
        }

        joint_pub.publish(joint_state);




        std_msgs::Float64 joint_po;
        ROS_INFO("hello");
        joint_po.data=joint_state.position[0];
        lf_jian_pu.publish(joint_po);
        joint_po.data=joint_state.position[1];
        lf_zhou_pu.publish(joint_po);
        joint_po.data=joint_state.position[2];
        lf_xi_pu.publish(joint_po);

        joint_po.data=joint_state.position[6];
        lb_jian_pu.publish(joint_po);
        joint_po.data=joint_state.position[7];
        lb_zhou_pu.publish(joint_po);
        joint_po.data=joint_state.position[8];
        lb_xi_pu.publish(joint_po);

        joint_po.data=joint_state.position[12];
        rf_jian_pu.publish(joint_po);
        joint_po.data=joint_state.position[13];
        rf_zhou_pu.publish(joint_po);
        joint_po.data=joint_state.position[14];
        rf_xi_pu.publish(joint_po);

        joint_po.data=joint_state.position[18];
        rb_jian_pu.publish(joint_po);
        joint_po.data=joint_state.position[19];
        rb_zhou_pu.publish(joint_po);
        joint_po.data=joint_state.position[20];
        rb_xi_pu.publish(joint_po);


        ts.header.stamp=ros::Time::now();
        point_base.header.stamp=ros::Time::now();
        point_base.point.x=0.00025*2*pi;
        point_base.point.y=0;
        point_base.point.z=0;

        geometry_msgs::PointStamped point_world;
        point_world=BUffer.transform(point_base,"world");

        ts.transform.translation.x=point_world.point.x;
        ts.transform.translation.y=point_world.point.y;
        ts.transform.translation.z=point_world.point.z;

        point_base.point.x=0.0;



        ts.transform.translation.x+=0.00025*2*pi;
        ts.transform.translation.y=0.0;
        ts.transform.translation.z=0.0805;

        qtn.setRPY(r_b,p_b,y_b);
        ts.transform.rotation.x=qtn.getX();
        ts.transform.rotation.y=qtn.getY();
        ts.transform.rotation.z=qtn.getZ();
        ts.transform.rotation.w=qtn.getW();
        broadcaster.sendTransform(ts);

        resu_last_l=result_l;
        resu_last_r=result_r;
        r.sleep();
    }
    }
    else if(cmd=='b')
    {
    //x=0.02*(t-sint);
    //y=0.02*(1-cost);
    for(int i=1;i<=20;i++)
    { 
        double t=2*pi*i/20;
        double x=0.005*(t-sin(t));
        double z=0.005*(1-cos(t));
        end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+0.005*2*pi-x;
        end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2]+z;
        int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);

        end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+x;
        end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2];
        int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);

        joint_state.header.stamp = ros::Time::now();

        for(int j=0;j<=23;j++)
        {
            joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
        }
        joint_pub.publish(joint_state);

        ts.header.stamp=ros::Time::now();
        point_base.header.stamp=ros::Time::now();
        point_base.point.x=-0.00025*2*pi;
        point_base.point.y=0;
        point_base.point.z=0;

        geometry_msgs::PointStamped point_world;
        point_world=BUffer.transform(point_base,"world");

        ts.transform.translation.x=point_world.point.x;
        ts.transform.translation.y=point_world.point.y;
        ts.transform.translation.z=point_world.point.z;

        point_base.point.x=0;

/*
        ts.transform.translation.x-=0.00025*2*pi;
        ts.transform.translation.y=0.0;
        ts.transform.translation.z=0.0805;
*/
        qtn.setRPY(r_b,p_b,y_b);
        ts.transform.rotation.x=qtn.getX();
        ts.transform.rotation.y=qtn.getY();
        ts.transform.rotation.z=qtn.getZ();
        ts.transform.rotation.w=qtn.getW();
        broadcaster.sendTransform(ts);

        resu_last_l=result_l;
        resu_last_r=result_r;
        r.sleep();
    }
    
    for(int i=1;i<=20;i++)
    {
        double t=2*pi*i/20;
        double x=0.005*(t-sin(t));
        double z=0.005*(1-cos(t));
        end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+x;
        end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2];
        int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);

        end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+0.005*2*pi-x;
        end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2]+z;
        int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);
    
        joint_state.header.stamp = ros::Time::now();

        for(int j=0;j<=23;j++)
        {
            joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
        }

        joint_pub.publish(joint_state);

        ts.header.stamp=ros::Time::now();
        point_base.header.stamp=ros::Time::now();
        point_base.point.x=-0.00025*2*pi;
        point_base.point.y=0;
        point_base.point.z=0;

        geometry_msgs::PointStamped point_world;
        point_world=BUffer.transform(point_base,"world");

        ts.transform.translation.x=point_world.point.x;
        ts.transform.translation.y=point_world.point.y;
        ts.transform.translation.z=point_world.point.z;

        point_base.point.x=0;
/*
        ts.transform.translation.x-=0.00025*2*pi;
        ts.transform.translation.y=0.0;
        ts.transform.translation.z=0.0805;
*/
        qtn.setRPY(r_b,p_b,y_b);
        ts.transform.rotation.x=qtn.getX();
        ts.transform.rotation.y=qtn.getY();
        ts.transform.rotation.z=qtn.getZ();
        ts.transform.rotation.w=qtn.getW();
        broadcaster.sendTransform(ts);

        resu_last_l=result_l;
        resu_last_r=result_r;
        r.sleep();
    }
    }
    else if(cmd=='c')
    {
    //x=0.02*(t-sint);
    //y=0.02*(1-cost);
    for(int i=1;i<=20;i++)
    { 
        double t=2*pi*i/20;
        double x=0.005*(t-sin(t));
        double z=0.005*(1-cos(t));
        end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+0.005*2*pi-x;
        end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2]+z;
        int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);

        end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+0.005*2*pi-x;
        end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2];
        int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);

        joint_state.header.stamp = ros::Time::now();

        for(int j=0;j<=23;j++)
        {
            joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
        }
        joint_pub.publish(joint_state);

        ts.header.stamp=ros::Time::now();
        y_b+=pi/(20*20*2);
        qtn.setRPY(r_b,p_b,y_b);
        ts.transform.rotation.x=qtn.getX();
        ts.transform.rotation.y=qtn.getY();
        ts.transform.rotation.z=qtn.getZ();
        ts.transform.rotation.w=qtn.getW();
        broadcaster.sendTransform(ts);

        resu_last_l=result_l;
        resu_last_r=result_r;
        r.sleep();
    }
    
    for(int i=1;i<=20;i++)
    {
        double t=2*pi*i/20;
        double x=0.005*(t-sin(t));
        double z=0.005*(1-cos(t));
        end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+x;
        end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2];
        int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);

        end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+x;
        end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2]+z;
        int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);
    
        joint_state.header.stamp = ros::Time::now();

        for(int j=0;j<=23;j++)
        {
            joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
        }

        joint_pub.publish(joint_state);

        ts.header.stamp=ros::Time::now();
        y_b+=pi/(20*20*2);
        qtn.setRPY(r_b,p_b,y_b);
        ts.transform.rotation.x=qtn.getX();
        ts.transform.rotation.y=qtn.getY();
        ts.transform.rotation.z=qtn.getZ();
        ts.transform.rotation.w=qtn.getW();
        broadcaster.sendTransform(ts);

        resu_last_l=result_l;
        resu_last_r=result_r;
        r.sleep();
    }
    }
    else if(cmd=='d')
    {
    //x=0.02*(t-sint);
    //y=0.02*(1-cost);
    for(int i=1;i<=20;i++)
    { 
        double t=2*pi*i/20;
        double x=0.005*(t-sin(t));
        double z=0.005*(1-cos(t));

        end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+x;
        end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2]+z;
        int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);

        end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+x;
        end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2];
        int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);

        joint_state.header.stamp = ros::Time::now();

        for(int j=0;j<=23;j++)
        {
            joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
        }
        joint_pub.publish(joint_state);

        ts.header.stamp=ros::Time::now();
        y_b-=pi/(20*20*2);
        qtn.setRPY(r_b,p_b,y_b);
        ts.transform.rotation.x=qtn.getX();
        ts.transform.rotation.y=qtn.getY();
        ts.transform.rotation.z=qtn.getZ();
        ts.transform.rotation.w=qtn.getW();
        broadcaster.sendTransform(ts);

        resu_last_l=result_l;
        resu_last_r=result_r;
        r.sleep();
    }
    
    for(int i=1;i<=20;i++)
    {
        double t=2*pi*i/20;
        double x=0.005*(t-sin(t));
        double z=0.005*(1-cos(t));
        end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+(0.005*2*pi-x);
        end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2];
        int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);

        end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+0.005*2*pi-x;
        end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2]+z;
        int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);
    
        joint_state.header.stamp = ros::Time::now();

        for(int j=0;j<=23;j++)
        {
            joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
        }

        joint_pub.publish(joint_state);

        ts.header.stamp=ros::Time::now();
        y_b-=pi/(20*20*2);
        qtn.setRPY(r_b,p_b,y_b);
        ts.transform.rotation.x=qtn.getX();
        ts.transform.rotation.y=qtn.getY();
        ts.transform.rotation.z=qtn.getZ();
        ts.transform.rotation.w=qtn.getW();
        broadcaster.sendTransform(ts);

        resu_last_l=result_l;
        resu_last_r=result_r;
        r.sleep();
    }
    }
    else if(cmd=='e')
    r.sleep();
    else;
    }
    return 0;  
}

